//import com.alderstone.multitouch.mac.touchpad.*;
import oscP5.*;
import netP5.*;
// import guicomponents.*;
import g4p_controls.*;
import processing.video.*;

//Touchpad touchpad;
Capture videoIn;
OscP5 osc;
NetAddress quadAddr;

// GOptionGroup optGroup1;
// GOption absAltButton, pAGLbutton, sAGLbutton, iAGLbutton;
// GOptionGroup optGroup2;
// GOption mCompassButton, IMUcompassButton;
// GButton motorTestButton, resetStabButton;


BufferedReader reader;

PFont font, mono;

int CMD_MIN = 900;
float CMD_YELLOW = 1250;
float CMD_RED = 1550;
int CMD_MAX = 1700;
float BAT_OFF = 471;
float BAT_WARN = 500;

boolean xyPadNeedsRedraw = true;
int x_accel = 0, y_accel = 0, z_accel = 0;
float xAccel = 0.5, yAccel = 0.5, zAccel = 0.5;
float xGyro = 0.5, yGyro = 0.5, zGyro = 0.5;
float agl = 0.0, maxAGL = 100;
boolean useMagCompass = true;

int altSelect = 0;


boolean have_map_info;
int [] map_data = new int[4];


int [] xyRect = {10, 64, 110, 110};
int [] stickRect = {40, 370, 110, 110};
int [] rudderRect = {40, 490, 110, 20};
int [] heightRect = {10, 370,  20, 110};
int [] zRect = {130, 64,  20, 110};
int [] cmd1Rect = {10, 190, 20, 100};
int [] cmd2Rect = {50, 190, 20, 100};
int [] cmd3Rect = {90, 190, 20, 100};
int [] cmd4Rect = {130, 190, 20, 100};
int [] xGyroRect = { 10, 10, 45, 45};
int [] yGyroRect = {60, 10, 45, 45};
int [] zGyroRect = {110, 10, 45, 45};
int [] compassRect = { 310, 410, 100, 100};
int [] horizonRect = { 310, 300, 100, 100};
int [] altRect = { 420, 300, 100, 100};
int [] tcRect = { 200, 410, 100, 100};
int [] vsiRect = {420, 410, 100, 100};
int [] speedRect = { 200, 300, 100, 100};
int [] battRect = {10, 375, 140, 20};
int [] vRateRect = {525, 420, 15, 80};
int [] sxRect = {525, 300, 15, 100};
int [] clockRect = {25, 410, 100, 100};
int [] rateRect = {550, 300, 100, 100};
int [] textRect = {680, 300, 340, 12};


color RED = color(255,0,0);
color YELLOW = color(255,255,0);
color DULL_YELLOW = color(200,200,0);
color GREEN = color(0,255,0);
color BLUE = color(0,255,0);
color BLACK = color(0,0,0);
color WHITE = color(255,255,255);

PImage theMap;

Attitude attitude;

int x_accelQ = 0;
int y_accelQ = 0;
int z_accelQ = -2048;
int x_rateQ = 0;
int y_rateQ = 0;
int z_rateQ = 0;
int altQ = 0;
int altGPS = 0;
int agl_PQ = 0;
int agl_SQ = 0;
int latQ = 0;
int lonQ = 0;
int v_northQ = 700;
int v_eastQ = 700;
int headingQ = 0;
int cmd1Q = 900;
int cmd2Q = 900;
int cmd3Q = 900;
int cmd4Q = 900;
int batteryQ = 550;
float IMUheadQ = 300;
float IMUpitchQ = 0;
float IMUrollQ = 0;
int IMUvX = 0;
int IMUvY = 0;
int IMUvZ = 0;


float stickP = 0.0;
float stickR = 0.0;
float rudder = 0.0;
float throttle = 0.0;
float startSP = 0;
float startSR = 0;
float startR = 0;
float startT = 0;

boolean touchControl = false;

XYpad accelXY;
Vslider accelZ;
Rotary gyroX, gyroY, gyroZ;
Vslider cmd1, cmd2, cmd3, cmd4;
Hslider battery;
AirSpeed aSpeed;
Compass compass;
Horizon horizon;
Altimeter altimeter;
Variometer vsi;
Vslider vertRate;
Vslider sonicAlt;
TurnCoord turnCoord;
Clock clock;
XYvector vGPSdisplay;

TextBox textbox;


void setup() {
  size(1060,520);
  frameRate(5);
  background(BLACK);
  
 attitude = new Attitude();
  
  // load imagedata for Altimeter, Variometer, & map
  theMap = loadImage("map.jpg");
  reader = createReader("mapInfo.txt");
  font = loadFont("SansSerif-14.vlw");
  mono = loadFont("Monaco-10.vlw");
  
  textbox = new TextBox(textRect, 15, mono, BLACK, WHITE);
  textbox.scrollText("This is a test!");
  
  // Read & Load Map Data
  String lineIn;
  
  for (int ctr = 0; ctr < 4; ctr++) {
    try {
      lineIn = reader.readLine();
    } catch (IOException e) {
      e.printStackTrace();
      lineIn = null;
    }
    if (lineIn != null) {
      map_data[ctr] = int(lineIn);
    }
  }
  
  // Instantiate 320x240 pixel video object
  videoIn = new Capture(this, "name=Built-in iSight,size=320x240,fps=5");
  videoIn.start();
  
  /* start oscP5, listening for incoming messages at port 9000 */
  osc = new OscP5(this, 9000);
  // Send OSC to quad @ port 8000
  quadAddr = new NetAddress("169.254.1.1",8000);
  
  // Plug in OSC method Callbacks

  osc.plug(this,"OSCacc","/acc");
  osc.plug(this,"OSCgyr","/gyr");
  osc.plug(this,"OSCalt","/alt");
  osc.plug(this,"OSCgps","/gps");
  osc.plug(this,"OSCpwr","/pwr");
  osc.plug(this,"OSCimu","/imu"); 
  
  // Instantiate the GUI elements
  createGUI();
  
  accelXY = new XYpad(xyRect, -4096.0, 4096.0);
  accelXY.setColor(YELLOW, BLACK);
  accelZ = new Vslider(zRect, -4096.0, 4096.0);
  accelZ.setColor(YELLOW, BLACK);
  gyroX = new Rotary(xGyroRect, -1300.0, 1300.0);
  gyroX.setColor(YELLOW, BLACK);
  gyroY = new Rotary(yGyroRect, -1300.0, 1300.0);
  gyroY.setColor(YELLOW, BLACK);
  gyroZ = new Rotary(zGyroRect, -1300.0, 1300.0);
  gyroZ.setColor(YELLOW, BLACK);
  cmd1 = new Vslider(cmd1Rect, float(CMD_MIN), float(CMD_MAX));
  cmd1.setColor(YELLOW, BLACK);
  cmd2 = new Vslider(cmd2Rect, float(CMD_MIN), float(CMD_MAX));
  cmd2.setColor(YELLOW, BLACK);
  cmd3 = new Vslider(cmd3Rect, float(CMD_MIN), float(CMD_MAX));
  cmd3.setColor(YELLOW, BLACK);
  cmd4 = new Vslider(cmd4Rect, float(CMD_MIN), float(CMD_MAX));
  cmd4.setColor(YELLOW, BLACK);
  battery = new Hslider(battRect, 0, 1024.0);
  battery.setColor(RED, BLACK);
  battery.setColor(YELLOW, BAT_OFF);
  battery.setColor(GREEN, BAT_WARN);
  aSpeed = new AirSpeed(speedRect, 100.0);
  compass = new Compass(compassRect);
  horizon = new Horizon(horizonRect);
  altimeter = new Altimeter(altRect);
  vsi = new Variometer(vsiRect);
  vertRate = new Vslider(vRateRect, -1000, 1000);
  sonicAlt = new Vslider(sxRect, 0, 650);
  turnCoord = new TurnCoord(tcRect);
  clock = new Clock(clockRect);
  clock.drawSecond = true;
  vGPSdisplay = new XYvector(rateRect, -1000, 1000);
  vGPSdisplay.setColor(WHITE, BLACK);
}

void draw() {
  background(0);
  sendStsReq();
  if (videoIn.available() == true) {
    videoIn.read();
  }
  set(200, 10, videoIn);
  
  if (float(theMap.height) / float(theMap.width) > 0.5625) {
    image(theMap, 550, 10, 280 * theMap.width / theMap.height, 280);
  } else {
    image(theMap, 550, 10, 500, 500 * theMap.height / theMap.width);
  }
  
  textFont(font);
  fill(BLACK);
  text(posString(latQ), 561, 26);
  text(posString(lonQ), 561, 46);
  text(altString(altGPS), 561, 66);
  fill(WHITE);
  text(posString(latQ), 560, 25);
  text(posString(lonQ), 560, 45);
  text(altString(altGPS), 560, 65);
  
  accelXY.draw(float(y_accelQ), float(-x_accelQ));
  accelZ.draw(-float(4096 + z_accelQ));
  gyroX.draw(float(x_rateQ));
  gyroY.draw(float(y_rateQ));
  gyroZ.draw(float(z_rateQ));
  cmd1.draw(float(cmd1Q));
  cmd2.draw(float(cmd2Q));
  cmd3.draw(float(cmd3Q));
  cmd4.draw(float(cmd4Q));
  battery.draw(float(batteryQ));
  aSpeed.draw(35.0);
  float h = IMUheadQ;
  //if (useMagCompass) h = float(headingQ) * PI / 1800;
  compass.draw(h);
  horizon.draw(IMUpitchQ, IMUrollQ);
  switch (altSelect) {
    case 0: {
      altimeter.draw(float(altQ));
      break;
    }
    case 1: {
      altimeter.draw(float(agl_PQ)*0.0328084);
      break;
    }
    case 2: {
      altimeter.draw(float(agl_SQ)*0.0328084);
      break;
    }
  }
  sonicAlt.draw(float(agl_SQ));
  vsi.draw(float(-IMUvZ)*(3.77e-5));
  vertRate.draw(float(-IMUvZ));
  turnCoord.draw(0.0, 0.0);
  vGPSdisplay.draw(float(IMUvX), float(IMUvY));
  clock.drawNow();
  textbox.draw();
  
  delay(100);
}

/*
void oscEvent(OscMessage theOscMessage) {
  // print the address pattern and the typetag of the received OscMessage 
  print("### received an osc message.");
  print(" addrpattern: "+theOscMessage.addrPattern());
  println(" typetag: "+theOscMessage.typetag());
  if (theOscMessage.getTypetagAsBytes()[0] == 's') {println(theOscMessage.get(0).stringValue());}
  if (theOscMessage.getTypetagAsBytes()[0] == 'i') {println(theOscMessage.get(0).intValue());}
}
*/

